﻿using Bettzueche.RLLibrary.Functions;
using System;
using System.Diagnostics;
using System.Threading.Tasks;
using System.Windows;

namespace AMEE1
{
    /// <summary>
    /// Simuliert eine visuelle Sensorschnittstelle, die die Bodenbeschaffenheit auf Unebenheiten analysiert. 
    /// </summary>
    /// <remarks>
    /// Initialisierung mit der <see cref="MapConfiguration">Kartenkonfiguration</see>, und einer
    /// <see cref="Bettzueche.RLLibrary.Functions.BumpinessFunction">"Weltfunktion"</see> f(x,y)=u, sowie Verteilung der
    /// simulierten Messpunkte je Abschnitt. Die Methode <see cref="SimuSensor#Messe(int,int)">Messe(int,int)</see> gibt
    /// zu einer Position (x,y) in der "Realwelt" die gemittelten Unebenheiten je Abschnitt.
    /// </remarks>
    /// <example>
    /// using Bettzueche.RLLibrary.Functions; // BumpinessFunction, Hilfsfunktionen
    /// namespace NSExample {
    ///     class Example 
    ///     {
    ///         static void DoExample() {
    ///             // Definiere Karten-Konfiguration
    ///             int anzSektoren = 4;
    ///             float[] spurRadien = { 0.5f, 1.0f }; // zwei Spuren
    ///             MapConfiguration weltConfig = new MapConfiguration(anzSektoren, spurRadien);
    ///             
    ///             // Simulation von Unebenheiten in einer "realen" Welt
    ///             BumpinessFunction realWelt = HilfsFunktionen.Weltfunktion2;
    /// 
    ///             SimuSensor sensor = new SimuSensor(weltConfig, realWelt);
    ///             
    ///             double xKoord = -5.0;
    ///             double yKoord = 10.0;
    ///             double[] unebenheiten = sensor.Messe(xKoord, yKoord);
    ///             
    ///             int abNr = weltConfig.GibAbschnittsnummer(spur: 1, sektor: 3);
    ///             double u7 = unebenheiten[abNr];
    ///             Console.WriteLine("Unebenheit in Abschnitt {0}: {1:N}", abNr, u7);
    ///         }
    ///         /* Ausgabe:
    ///          * Unebenheit in Abschnitt 7: 0,13
    ///          */
    ///     }
    /// }
    /// 
    /// </example>
    class SimuSensor
    {
        #region Fields

        /// <summary>Konfiguration der Egozentrischen Roboterkarte.</summary>
        internal readonly MapConfiguration MapConf;

        BumpinessFunction _weltSensor;
        double[] _unebenheiten;
        Vector[] _messPunkte;
        int _abschnitteGesamt;
        int _messungenJeAbschnitt;

        #endregion


        #region Properties

        /// <summary>
        /// Gets the Anzahl Messwinkel je Sektor.
        /// </summary>
        /// <value>
        /// The anzahl messwinkel je sektor.
        /// </value>
        internal int AnzahlMesswinkelJeSektor { get; private set; }

        /// <summary>
        /// Gets the Anzahl Messreihen je Spur.
        /// </summary>
        /// <value>
        /// The anzahl messreihen je spur.
        /// </value>
        internal int AnzahlMessreihenJeSpur { get; private set; }

        #endregion


        #region Konstruktor(en)


        /// <summary>
        /// Initializes a new instance of the <see cref="SimuSensor" /> class.
        /// </summary>
        /// <param name="weltConfig">The welt configuration.</param>
        /// <param name="realWelt">Die "Weltfunktion", die zu reellen x,y Werten ein reeles "Unebenheitsmaß" wiedergibt.</param>
        /// <param name="messReihenJeSpur">Anzahl Messreihen je Spur. Optional; Standard = 3</param>
        /// <param name="messWinkelJeSpur">Anzahl Messwinkel (-zeilen) je Spur. Optional; Standard = 5</param>
        /// <exception cref="System.ArgumentNullException">weltConfig or realWelt</exception>
        /// <exception cref="System.ArgumentException">
        /// Wert kleiner 1; messReihenJeSpur
        /// or
        /// Wert kleiner 1; messWinkelJeSpur
        /// </exception>
        /// <remarks>'messReihenJeSpur' und 'messWinkelJeSpur' werden zur Ermittlung der Messpunkte bei der
        /// Abbildung von der Realwelt auf diese Karte genutzt. Pro Abschnitt (Sektor,Spur) werden dann 
        /// 'messReihenJeSpur' * 'messWinkelJeSpur' Messpunkte zu einem Unebenheitswert für den Abschnitt verrechnet.</remarks>
        public SimuSensor(MapConfiguration weltConfig, BumpinessFunction realWelt, int messReihenJeSpur=3, int messWinkelJeSpur=5) {
            // Vorbedingungen
            if (weltConfig == null)
                throw new ArgumentNullException("weltConfig");
            if (realWelt == null)
                throw new ArgumentNullException("realWelt");
            if (messReihenJeSpur <= 0)
                throw new ArgumentException("Wert kleiner 1", "messReihenJeSpur");
            if (messWinkelJeSpur <= 0)
                throw new ArgumentException("Wert kleiner 1", "messWinkelJeSpur");
            AnzahlMessreihenJeSpur = messReihenJeSpur;
            AnzahlMesswinkelJeSektor = messWinkelJeSpur;
            MapConf = weltConfig;
            _abschnitteGesamt = MapConf.Sektoren * MapConf.Spuren; // z.B. 8*2 = 16 Abschnitte
            _messungenJeAbschnitt = AnzahlMesswinkelJeSektor * AnzahlMessreihenJeSpur; // z.B. 5*3 = 15 Messungen je Abschnitt
            _weltSensor = realWelt;
            init(weltConfig.SpurRadien);
        }

        #endregion


        #region Methods

        /// <summary>
        /// Erstellt (misst) zur gegebenen Realwelt-Koordinate die interne (egozentrische) Kartenrepräsentation.<para>
        /// Vordefinierte Messpunkte (lokales Array) werden mit der gegebenen Koordinate verrechnet und als Parameter der
        /// Weltfunktion übergeben. Je 15 Werte pro Abschnitt in der lokalen Kartenrepräsentation werden zu einem 
        /// Durchschnittswert gemittelt und eingetragen.</para>
        /// </summary>
        /// <param name="realWeltX">The real welt X.</param>
        /// <param name="realWeltY">The real welt Y.</param>
        /// <returns>Array von Unebenheiten, indiziert nach Abschnittsnummer (<see cref="MapConfiguration#GibAbschnittsnummer(int,int)"/>).</returns>
        public double[] Messe(double realWeltX, double realWeltY) {
            double[] result = new double[MapConf.Abschnitte];

            // Mittelwert aus 15 Messpunkten je Abschnitt berechnen und eintragen:
            //for(int = 0; s < abschnitteGesamt; s++)
            Parallel.For(0, _abschnitteGesamt, sktrNr => {
                double sum = 0;
                for (int i = 0; i < _messungenJeAbschnitt; i++) {
                    Vector messPkt = _messPunkte[sktrNr * _messungenJeAbschnitt + i];
                    sum += _weltSensor(realWeltX + messPkt.X, realWeltY + messPkt.Y);
                }
                double mittelwert = sum / _messungenJeAbschnitt;
                // Eintragen:
                int abNr = MapConf.GibAbschnittsnummer(sktrNr / MapConf.Sektoren, sktrNr % MapConf.Sektoren);
                result[abNr] = mittelwert;
            });
            return result;
        }

        /// <summary>
        /// Gibt das Unebenheitsmaß am spezifizerten Punkt in der "Realwelt."
        /// </summary>
        /// <param name="realWeltX">X-Koordinate</param>
        /// <param name="realWeltY">Y-Koordinate</param>
        /// <returns>Unebenheitsmaß am spezifizerten Punkt</returns>
        public double MessePunkt(double realWeltX, double realWeltY) {
            return _weltSensor(realWeltX, realWeltY);
        }

        #endregion


        #region Helpers

        /// <summary>
        /// Initialisierung. Wird vom Konstruktor aufgerufen.<para>
        /// Pro Abschnitt werden 15 Messkoordinaten ermittelt.</para><para>
        /// Diese Koordinaten werden von der Messfunktion genutzt, um mittels der Weltfunktion einen
        /// durchschnittlichen Unebenheitswert für jeden Abschnitt zu berechnen.</para>
        /// </summary>
        void init(float[] radien) {
            checkInitPreConditions(radien);
            int sektorenAnzahl = MapConf.Sektoren;
            int spurenAnzahl = MapConf.Spuren;

            _unebenheiten = new double[sektorenAnzahl * spurenAnzahl];

            int abschnitteGesamt = sektorenAnzahl * spurenAnzahl; // 8*2 = 16 Abschnitte
            int messungenJeAbschnitt = AnzahlMesswinkelJeSektor * AnzahlMessreihenJeSpur; // 5*3 = 15 Messungen je Abschnitt
            _messPunkte = new Vector[abschnitteGesamt * messungenJeAbschnitt]; // 240 Messpunkte gesamt

            float[][] spurenMessradien = initMessRadien(radien);

            // Jeder Sektor hat 45° Winkel. 5 Messwinkel je Sektor auf 1/10, 3/10, 1/2, 7/10, 9/10
            double sektorenWinkel = 2 * Math.PI / sektorenAnzahl; // bspw. 45° bei 8 Sektoren

            int idxOffset01 = messungenJeAbschnitt * sektorenAnzahl; // 5*3*8 = 120
            int winkelGesamt = sektorenAnzahl * AnzahlMesswinkelJeSektor; // 8*5 = 40
            double winkelOffset = sektorenWinkel / AnzahlMesswinkelJeSektor; // Messwinkelabstand in rad (bspw. 1/5 von 45° = 9°)
            double winkelTmp = (-sektorenWinkel / 2) + winkelOffset / 2; // Startwinkel Richtung Ost bzw x-Achse (bspw. -18°)

            // Berechnung der Messpunkte Koordinaten. Je 3*5=15, beginnend bei Spur0/Ost, Spur0/NO,...,Spur1/O,... 
            /*  NO-0       O-0
             * ... 17 | 14 11 ... 2 |
             * ... 16 | 13 10 ... 1 |
             * ... 15 | 12  9 ... 0 |
             */
            Parallel.For(0, winkelGesamt, w =>   // w = 0..39
            {
                for (int reihe = 0; reihe < AnzahlMessreihenJeSpur; reihe++) // j = 0..2
                {
                    int index = AnzahlMessreihenJeSpur * w + reihe;

                    for (int spur = 0; spur < spurenAnzahl; spur++) {
                        double x = spurenMessradien[spur][reihe] * Math.Cos(winkelTmp);
                        double y = spurenMessradien[spur][reihe] * Math.Sin(winkelTmp);
                        Vector koord = new Vector { X = x, Y = y };
                        _messPunkte[index + (spur * idxOffset01)] = koord;
                    }
                }
                // winkel verschieben:
                winkelTmp += winkelOffset;

            });
        }

        /// <summary>
        /// Checks the init pre conditions.
        /// </summary>
        /// <param name="radien">The radien.</param>
        /// <exception cref="RLProject1.AMEE1Exception">
        /// ArgumentNullException(radien)
        /// or
        /// Anzahl radien ist kleiner als SpurenAnzahl
        /// or
        /// AnzahlMesswinkelJeSektor ist kleiner 0.
        /// </exception>
        /// <exception cref="System.ArgumentNullException">radien</exception>
        void checkInitPreConditions(float[] radien) {
            //Vorbedingungen
            if (MapConf.Sektoren <= 0) {
                throw new AMEE1Exception("Initialisierungsfehler von WorldImage. SektorenAnzahl ist nicht positiv: " + MapConf.Sektoren);
            }
            int spurenAnzahl = MapConf.Spuren;
            if (spurenAnzahl <= 0) {
                throw new AMEE1Exception("Initialisierungsfehler von WorldImage. SpurenAnzahl ist nicht positiv " + spurenAnzahl);
            }
            if (radien == null) {
                throw new AMEE1Exception("Initialisierungsfehler von WorldImage. Siehe innere Ausnahme für mehr Details", new ArgumentNullException("radien"));
            }
            if (radien.Length < spurenAnzahl) {
                throw new AMEE1Exception("Initialisierungsfehler von WorldImage (Inkonsistenz). Anzahl radien ist kleiner als SpurenAnzahl");
            }
            Debug.WriteLineIf(radien.Length > spurenAnzahl, "Inkonsistenz bei Initialisierung von WorldImage redien.Length > SpurenAnzahl.", "Warnung");
            if (AnzahlMessreihenJeSpur <= 0) {
                Debug.WriteLine("Inkonsistenz bei Initialisierung von WorldImage. AnzahlMesrreihenJeSpur = " + AnzahlMessreihenJeSpur + ". Wert wurde auf radien.Length gesetzt!", "Warnung");
                AnzahlMessreihenJeSpur = radien.Length;
            }
            Debug.WriteIf(0 == AnzahlMessreihenJeSpur, "Inkonsistenz bei Initialisierung von WorldImage. AnzahlMesrreihenJeSpur = 0.", "Warnung");
            if (AnzahlMesswinkelJeSektor < 0) {
                throw new AMEE1Exception("Initialisierungsfehler von WorldImage (Inkonsistenz). AnzahlMesswinkelJeSektor ist kleiner 0.");
            }
            Debug.WriteLineIf(0 == AnzahlMesswinkelJeSektor, "Inkonsistenz bei Initialisierung von WorldImage. AnzahlMesswinkelJeSektor = 0.", "Warnung");
        }

        /// <summary>
        /// Initialisiert nach Spuren sortiert die jeweiligen Messradien (bezogen auf den Ursprung).<para>
        /// Je Spur werden die 'AnzahlMessreihenJeSpur' gleichmäßig mit Abstand 1/AnzahlMessreihenJeSpur der Spurenbreite in der Spur Verteilt</para>
        /// </summary>
        /// <param name="radien">Die Radien der Spuren (bezogen auf den Ursprung). Innersten Radius zuerst.</param>
        /// <returns>Nach Spuren sortierte Messradien (bezogen auf den Ursprung). Innerster Radius hat jeweils Index 0.</returns>
        float[][] initMessRadien(float[] radien) {
            int spurenAnzahl = MapConf.Spuren;
            float[][] spurenMessradien = new float[spurenAnzahl][];

            float teiler = 1f / (2f * AnzahlMessreihenJeSpur);
            float deltaTeiler = 1f / AnzahlMessreihenJeSpur;
            float r1 = 0f;
            int spur = 0;
            do {
                float r2 = radien[spur];
                float laenge = r2 - r1;
                float[] spurXMessradien = new float[AnzahlMessreihenJeSpur];// { r1 + (laenge * (0.5f - 1 / 3f)), r1 + (0.5f * laenge), r1 + (laenge * (0.5f + 1 / 3f)) };

                for (int reihe = 0; reihe < AnzahlMessreihenJeSpur; reihe++) {
                    spurXMessradien[reihe] = r1 + (laenge * teiler);
                    teiler += deltaTeiler;
                }
                spurenMessradien[spur] = spurXMessradien;
                r1 = r2;
                spur++;
            } while (spur < spurenAnzahl);

            return spurenMessradien;
        }

        #endregion
    }
}
